// --------------------------------------------------------------------------------------------------------------------
// <copyright file="RotationComparisonTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Class for tests comparing different rotation method against one another
    /// </summary>
    [TestClass]
    public class RotationComparisonTest
    {
        /// <summary>
        /// Test/compare homogenous transforms and quaternions on a single rotation
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void SingleRotationTest()
        {
            /* For test case consider two coordinate frames A and B, where B is derived from A by rotating -90deg around Z-A */

            // create a rotation only homogenous transform to rotate from frame B to Frame A (i.e. rotates by 90deg around Z-B)
            // set elements manually
            Matrix4 baseLineHT = HomogenousTransform.CreateFromTranslation(new Vector3(0, 0, 0));

            baseLineHT[0, 0] = 0.0;
            baseLineHT[1, 1] = 0.0;
            baseLineHT[2, 2] = 0.0;

            baseLineHT[1, 0] = -1.0;
            baseLineHT[0, 1] = 1.0;
            baseLineHT[2, 2] = 1.0;

            Vector3 notHungarianXAxis = new Vector3(1.0, 0.0, 0.0);
            Vector3 notHungarianYAxis = new Vector3(0.0, 1.0, 0.0);
            Vector3 notHungarianZAxis = new Vector3(0.0, 0.0, 1.0);

            // create same homogenous transform and quaternion equivalent from axis angle
            // Coordiante axis transformation from A to B is -90Z
            // Which implies transformation of a point in B to a point in A is -90Z (coordinate and point transforms are inverse of eachother).
            Matrix4 axisAngleHT =
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(notHungarianZAxis, -MathConstants.PIOverTwo));
            Quaternion axisAngleQuat =
                Quaternion.FromAxisAngle(new AxisAngle(notHungarianZAxis, -MathConstants.PIOverTwo));

            Pose poseB = new Pose(new Vector3(0.0, 0.0, 0.0), axisAngleQuat);

            Matrix4 poseHT = HomogenousTransform.CreateFromPose(poseB);

            // transforming unit vectors in frame B to A
            Assert.AreEqual(-notHungarianYAxis, HomogenousTransform.Apply(baseLineHT, notHungarianXAxis));
            Assert.AreEqual(-notHungarianYAxis, HomogenousTransform.Apply(axisAngleHT, notHungarianXAxis));
            Assert.AreEqual(-notHungarianYAxis, HomogenousTransform.Apply(poseHT, notHungarianXAxis));
            Assert.AreEqual(-notHungarianYAxis, Quaternion.Rotate(axisAngleQuat, notHungarianXAxis));

            Assert.AreEqual(notHungarianXAxis, HomogenousTransform.Apply(baseLineHT, notHungarianYAxis));
            Assert.AreEqual(notHungarianXAxis, HomogenousTransform.Apply(axisAngleHT, notHungarianYAxis));
            Assert.AreEqual(notHungarianXAxis, HomogenousTransform.Apply(poseHT, notHungarianYAxis));
            Assert.AreEqual(notHungarianXAxis, Quaternion.Rotate(axisAngleQuat, notHungarianYAxis));

            Assert.AreEqual(notHungarianZAxis, HomogenousTransform.Apply(baseLineHT, notHungarianZAxis));
            Assert.AreEqual(notHungarianZAxis, HomogenousTransform.Apply(axisAngleHT, notHungarianZAxis));
            Assert.AreEqual(notHungarianZAxis, HomogenousTransform.Apply(poseHT, notHungarianZAxis));
            Assert.AreEqual(notHungarianZAxis, Quaternion.Rotate(axisAngleQuat, notHungarianZAxis));
        }

        /// <summary>
        /// Test/compare homogenous transforms and quaternions on a compound rotation
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CompoundRotationTest()
        {
            /* For test case consider two coordinate frames A and B, where B is derived from A by rotating +90deg around X-A and then +90deg around Z-A */

            // create a rotation only homogenous transform to rotate from frame B to Frame A (i.e. rotates by 90deg around Z-B)
            // set elements manually
            Matrix4 baseLineHT = HomogenousTransform.CreateFromTranslation(new Vector3(0.0, 0.0, 0.0));

            baseLineHT[0, 0] = 0.0;
            baseLineHT[1, 1] = 0.0;
            baseLineHT[2, 2] = 0.0;

            baseLineHT[1, 0] = 1.0;
            baseLineHT[2, 1] = 1.0;
            baseLineHT[0, 2] = 1.0;

            Vector3 notHungarianXAxis = new Vector3(1.0, 0.0, 0.0);
            Vector3 notHungarianYAxis = new Vector3(0.0, 1.0, 0.0);
            Vector3 notHungarianZAxis = new Vector3(0.0, 0.0, 1.0);

            // create same homogenous transform and quaternion equivalent from axis angle
            // Coordiante axis transformation from A to B is 90X then 90Z in frame A
            // Which implies transformation of a point in B to a point in A is 90X then 90Z in frame A (coordinate and point transforms are inverse of eachother).
            Matrix4 axisAngleHT =
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(notHungarianZAxis, MathConstants.PIOverTwo))
                * HomogenousTransform.CreateFromAxisAngle(new AxisAngle(notHungarianXAxis, MathConstants.PIOverTwo));
            Quaternion axisAngleQuat =
                Quaternion.FromAxisAngle(new AxisAngle(notHungarianZAxis, MathConstants.PIOverTwo))
                * Quaternion.FromAxisAngle(new AxisAngle(notHungarianXAxis, MathConstants.PIOverTwo));

            Pose poseB = new Pose(new Vector3(0.0, 0.0, 0.0), axisAngleQuat);

            Matrix4 poseHT = HomogenousTransform.CreateFromPose(poseB);

            // transforming unit vectors in frame B to A
            Assert.AreEqual(notHungarianYAxis, HomogenousTransform.Apply(baseLineHT, notHungarianXAxis));
            Assert.AreEqual(notHungarianYAxis, HomogenousTransform.Apply(axisAngleHT, notHungarianXAxis));
            Assert.AreEqual(notHungarianYAxis, HomogenousTransform.Apply(poseHT, notHungarianXAxis));
            Assert.AreEqual(notHungarianYAxis, Quaternion.Rotate(axisAngleQuat, notHungarianXAxis));

            Assert.AreEqual(notHungarianZAxis, HomogenousTransform.Apply(baseLineHT, notHungarianYAxis));
            Assert.AreEqual(notHungarianZAxis, HomogenousTransform.Apply(axisAngleHT, notHungarianYAxis));
            Assert.AreEqual(notHungarianZAxis, HomogenousTransform.Apply(poseHT, notHungarianYAxis));
            Assert.AreEqual(notHungarianZAxis, Quaternion.Rotate(axisAngleQuat, notHungarianYAxis));

            Assert.AreEqual(notHungarianXAxis, HomogenousTransform.Apply(baseLineHT, notHungarianZAxis));
            Assert.AreEqual(notHungarianXAxis, HomogenousTransform.Apply(axisAngleHT, notHungarianZAxis));
            Assert.AreEqual(notHungarianXAxis, HomogenousTransform.Apply(poseHT, notHungarianZAxis));
            Assert.AreEqual(notHungarianXAxis, Quaternion.Rotate(axisAngleQuat, notHungarianZAxis));
        }
    }
}
